import math as m

import sys
from typing import Optional, Union

import numpy as np
import sympy as sp

# from rich import print
from scipy.linalg import lstsq

from tma.utils import settings as s
from tma.utils.custom_types import t_line, t_lines, t_point


def is_ordered(numbers: Union[list, tuple]) -> bool:
    increasing = all(numbers[i] <= numbers[i + 1] for i in range(len(numbers) - 1))
    decreasing = all(numbers[i] >= numbers[i + 1] for i in range(len(numbers) - 1))
    return increasing or decreasing


def get_angle_between_lines(a1, a2, b1):
    """
    line01 is defined by it's heading a1, a2
    line02's heading b1
    get the samller angle between them
    """
    res = min(abs(a1 - b1), abs(a2 - b1))
    if res > 90:
        res = 180 - res
    return res


def get_best_adjR(x: list[float], y: list[float], top_degree: int = 8) -> int:
    results = {}
    for degree in range(1, top_degree):
        coeffs = np.polyfit(x, y, degree)
        p = np.poly1d(coeffs)
        yhat = p(x)
        ybar = np.sum(y) / len(y)
        ssreg = np.sum((yhat - ybar) ** 2)
        sstot = np.sum((y - ybar) ** 2)
        results[degree] = 1 - (((1 - (ssreg / sstot)) * (len(y) - 1)) / (len(y) - degree - 1))

    for k, v in results.items():
        if k == 1:
            best_degree = k
            best_v = v
            continue
        if v >= best_v:
            best_degree = k
            best_v = v
        else:
            break

    return best_degree


def get_distance_between_lines(a_line, b_line):
    dist1 = cal_perpendicular_distance(a_line[:2], b_line)
    dist2 = cal_perpendicular_distance(a_line[2:], b_line)
    dist3 = cal_perpendicular_distance(b_line[:2], a_line)
    dist4 = cal_perpendicular_distance(b_line[2:], a_line)
    return max(dist1, dist2, dist3, dist4)


class HoughBundler:
    def __init__(self, min_distance: float = 2.0, min_angle: float = 0.3):
        self.min_distance = min_distance
        self.min_angle = min_angle

    def get_orientation(self, line):
        orientation = m.atan2(abs((line[3] - line[1])), abs((line[2] - line[0])))
        return m.degrees(orientation)

    def check_is_line_different(self, line_1, groups, min_distance_to_merge, min_angle_to_merge):
        for group in groups:
            for line_2 in group:
                if self.get_distance(line_2, line_1) < min_distance_to_merge:
                    orientation_1 = self.get_orientation(line_1)
                    orientation_2 = self.get_orientation(line_2)
                    if abs(orientation_1 - orientation_2) < min_angle_to_merge:
                        group.append(line_1)
                        return False
        return True

    def distance_point_to_line(self, point, line):
        px, py = point
        x1, y1, x2, y2 = line

        def line_magnitude(x1, y1, x2, y2):
            line_magnitude = m.sqrt(m.pow((x2 - x1), 2) + m.pow((y2 - y1), 2))
            return line_magnitude

        lmag = line_magnitude(x1, y1, x2, y2)
        if lmag < 0.00000001:
            distance_point_to_line = 9999
            return distance_point_to_line

        u1 = ((px - x1) * (x2 - x1)) + ((py - y1) * (y2 - y1))
        u = u1 / (lmag * lmag)

        if (u < 0.00001) or (u > 1):
            # // closest point does not fall within the line segment, take the shorter distance
            # // to an endpoint
            ix = line_magnitude(px, py, x1, y1)
            iy = line_magnitude(px, py, x2, y2)
            if ix > iy:
                distance_point_to_line = iy
            else:
                distance_point_to_line = ix
        else:
            # Intersecting point is on the line, use the formula
            ix = x1 + u * (x2 - x1)
            iy = y1 + u * (y2 - y1)
            distance_point_to_line = line_magnitude(px, py, ix, iy)

        return distance_point_to_line

    def get_distance(self, a_line, b_line):
        dist1 = self.distance_point_to_line(a_line[:2], b_line)
        dist2 = self.distance_point_to_line(a_line[2:], b_line)
        dist3 = self.distance_point_to_line(b_line[:2], a_line)
        dist4 = self.distance_point_to_line(b_line[2:], a_line)

        return min(dist1, dist2, dist3, dist4)

    def merge_lines_into_groups(self, lines):
        groups = []  # all lines groups are here
        # first line will create new group every time
        groups.append([lines[0]])
        # if line is different from existing gropus, create a new group
        for line_new in lines[1:]:
            if self.check_is_line_different(line_new, groups, self.min_distance, self.min_angle):
                groups.append([line_new])

        return groups

    def merge_line_segments(self, lines):
        orientation = self.get_orientation(lines[0])

        if len(lines) == 1:
            return np.block([[lines[0][:2], lines[0][2:]]])

        points = []
        for line in lines:
            points.append(line[:2])
            points.append(line[2:])
        if 45 < orientation <= 90:
            # sort by y
            points = sorted(points, key=lambda point: point[1])
        else:
            # sort by x
            points = sorted(points, key=lambda point: point[0])

        return np.block([[points[0], points[-1]]])

    def process_lines(self, lines):
        lines_horizontal = []
        lines_vertical = []

        # for line_i in [l[0] for l in lines]:
        for line_i in lines:
            orientation = self.get_orientation(line_i)
            # if vertical
            if 45 < orientation <= 90:
                lines_vertical.append(line_i)
            else:
                lines_horizontal.append(line_i)

        lines_vertical = sorted(lines_vertical, key=lambda line: line[1])
        lines_horizontal = sorted(lines_horizontal, key=lambda line: line[0])
        merged_lines_all = []

        # for each cluster in vertical and horizantal lines leave only one line
        for i in [lines_horizontal, lines_vertical]:
            if len(i) > 0:
                groups = self.merge_lines_into_groups(i)
                merged_lines = []
                for group in groups:
                    merged_lines.append(self.merge_line_segments(group))
                merged_lines_all.extend(merged_lines)

        data = [list(tuple(i[0][0]) + tuple((i[0][1]))) for i in merged_lines_all]
        return data


def get_vertical_line(line: t_line) -> t_line:
    """return a line vertical to provided line, and pass (x1, y1)"""
    x1, y1, x2, y2 = line
    if x1 == x2:  # Line is vertical
        return [x1, y1, x1 + 100, y1]
    else:
        m = (y2 - y1) / (x2 - x1)  # Calculate the slope of the line
        if m == 0:  # Line is horizontal
            # Return a vertical line passing through the x-coordinate of the line
            return [x1, 0, x1, 100]
        else:
            vertical_slope = -1 / m  # Calculate the slope of the vertical line
            # Choose a point on the original line and extend the vertical line to make it long enough
            x2 = x1 + 100
            y2 = y1 + 100 * vertical_slope
            # Return the vertical line represented by its endpoints
            return [x1, y1, x2, y2]


def get_vertical_line_slope(line: t_line) -> Optional[float]:
    x1, y1, x2, y2 = line
    if x1 == x2:  # Line is vertical
        return None  # Vertical line slope is undefined
    else:
        m = (y2 - y1) / (x2 - x1)  # Calculate the slope of the line
        if m == 0:  # Line is horizontal
            return 0  # Vertical line slope is 0
        else:
            vertical_slope = -1 / m  # Calculate the slope of the vertical line
            return vertical_slope


def if_bl_spread_over_y_axis(lines: t_lines) -> tuple[bool, int]:
    """
    check if bearing lines spread over vertical line
    check if all line slope is positive or negative
    """
    slopes = []
    for line in lines:
        x1, y1, x2, y2 = line
        if x2 == x1:
            slopes.append(sys.maxsize - 10)
        else:
            slopes.append((y2 - y1) / (x2 - x1))

    headings = [max(cal_line_angle(line)) for line in lines]
    if any([i > 360 - s.Vertical_bl_offset for i in headings]) and any([
        i < 180 + s.Vertical_bl_offset for i in headings
    ]):
        cross = True
    else:
        cross = False
    if all(i >= 0 for i in slopes):
        ret = (cross, 1)
    elif all(i <= 0 for i in slopes):
        ret = (cross, -1)
    else:
        ret = (cross, 0)
    return ret


def cal_average_slope(lines: t_lines) -> Union[float, int]:
    slopes = [(y2 - y1) / (x2 - x1) for x1, y1, x2, y2 in lines]
    if if_bl_spread_over_y_axis(lines)[0]:
        positive_slopes = [i for i in slopes if i > 0]
        negative_slopes = [i for i in slopes if i < 0]
        p = sum(positive_slopes) / len(positive_slopes)
        n = sum(negative_slopes) / len(negative_slopes)
        if p >= abs(n):
            average_slope = -(p + n)
        else:
            average_slope = p + abs(n)
    else:
        average_slope = sum(slopes) / len(slopes)

    return average_slope


def calculate_line_intersection_center(lines: t_lines):
    all_points = []
    for i in range(len(lines) - 1):
        for j in range(i + 1, len(lines)):
            x, y = find_intersection_point(lines[i], lines[j])
            all_points.append(sp.Point(x, y, evaluate=False))

    return sp.centroid(*all_points).coordinates


def cal_intersection_center(lines: t_lines, line_format: int = 4) -> tuple[Union[int, float], Union[int, float]]:
    A = np.zeros((len(lines), 3))

    if line_format == 4:
        for i, line in enumerate([(i[0], i[1]), (i[2], i[3])] for i in lines):
            x1, y1 = line[0]
            x2, y2 = line[1]
            A[i, :] = y2 - y1, x1 - x2, x2 * y1 - x1 * y2
    else:
        for i, line in enumerate(lines):
            x1, y1 = line[0]
            x2, y2 = line[1]
            A[i, :] = y2 - y1, x1 - x2, x2 * y1 - x1 * y2

    for index, i in enumerate(A):
        if any(np.isinf(i)) or any(np.isnan(i)):
            A = np.delete(A, index, axis=0)

    X, _, _, _ = lstsq(A[:, :2], -A[:, 2])

    return float(X[0]), float(X[1])


def alter_coordinate_origin(lines: t_lines, image_height: Union[int, float]) -> t_lines:
    """alter coordinate origin and sort point by x-axis
    remove line vertical to x-axis

    Args:
        lines (t_lines):
        height (Union[int,float]):

    Returns:
         t_lines
    """
    result: t_lines = []
    for line in lines:
        x1, y1, x2, y2 = line
        y1 = image_height - y1
        y2 = image_height - y2
        if x1 > x2:
            result.append([x2, y2, x1, y1])
        else:
            result.append([x1, y1, x2, y2])

    return result


def cal_line_angle(line: t_line) -> tuple[float, float]:
    x1, y1, x2, y2 = line
    if (x2 > x1 and y2 > y1) or (x2 < x1 and y2 < y1):
        res = (
            m.atan(abs(x1 - x2) / abs(y1 - y2)),
            m.atan(abs(x1 - x2) / abs(y1 - y2)) + m.pi,
        )
    elif (x2 < x1 and y2 > y1) or (x2 > x1 and y2 < y1):
        res = (
            2 * m.pi - m.atan(abs(x1 - x2) / abs(y1 - y2)),
            m.pi - m.atan(abs(x1 - x2) / abs(y1 - y2)),
        )
    elif x1 == x2:
        res = 0, m.pi
    elif y1 == y2:
        res = 3 * m.pi / 2, m.pi / 2

    return tuple(m.degrees(i) for i in res)


def find_intersection_point(line_A: t_line, line_B: t_line) -> t_point:
    """get the cross_point of two lines"""
    x1, y1, x2, y2 = line_A
    x3, y3, x4, y4 = line_B

    # Calculate the coefficients for the equations of the lines
    a1 = y2 - y1
    b1 = x1 - x2
    c1 = (y1 * x2) - (y2 * x1)

    a2 = y4 - y3
    b2 = x3 - x4
    c2 = (y3 * x4) - (y4 * x3)

    # Calculate the determinant
    determinant = (a1 * b2) - (a2 * b1)

    # Calculate the cross point coordinates
    if determinant != 0:
        cross_x: float = ((b1 * c2) - (b2 * c1)) / determinant
        cross_y: float = ((a2 * c1) - (a1 * c2)) / determinant
        return [cross_x, cross_y]
    else:
        # @TODO
        raise Exception(f"Iput lines is parallel: ${line_A}, ${line_B}")


def cal_perpendicular_distance(point: t_point, line: t_line):
    """calculate perpendicular distance between a point and a line"""
    x1, y1, x2, y2 = line
    x0, y0 = point
    numerator = abs((x2 - x1) * (y1 - y0) - (x1 - x0) * (y2 - y1))
    denominator = m.sqrt((x2 - x1) ** 2 + (y2 - y1) ** 2)
    distance = numerator / denominator
    return distance


def distance_between_points(p1: t_point, p2: t_point) -> float:
    return m.dist(p1, p2)


def find_vertical_point(line: t_line, p1: t_point) -> tuple[float, float]:
    """
    give a line A and point p1, get the point p2 on line,
    where line(p1, p2) is vertical line A
    """
    x1, y1 = p1
    (x2, y2, x3, y3) = line

    # Calculate the slope of line01
    slope_line01 = (y3 - y2) / (x3 - x2)

    # The slope of the line perpendicular to line01 is the negative reciprocal of the slope of line01
    slope_perpendicular = -1 / slope_line01 if slope_line01 != 0 else float("inf")

    # Using slope and p1 we can find the equation of the perpendicular line that passess through p1
    # Then we find the intersection of this perpendicular line with line01
    if slope_perpendicular == float("inf"):
        x2, y2 = find_intersection_point([x1, y1, x1, y1 + 10], line)
    else:
        # y = m*x + b
        b = y1 - slope_perpendicular * x1

        x2, y2 = find_intersection_point([x1, y1, x1 + 10, (slope_perpendicular * (x1 + 10) + b)], line)

    return (x2, y2)


def divide_track_by_bl(line: t_line, bl_time_tick: list[int]) -> list[t_point]:
    """
    divide track into n segments corresponds to the bearing line spread
    """
    first_idx = bl_time_tick[0]
    bl_time_tick = [i - first_idx for i in bl_time_tick]
    x1, y1, x2, y2 = line
    n = bl_time_tick[-1]
    # print(f"n: {list(range(1,n))}")

    equally_divided_points = [(x1, y1)]
    for i in range(1, n):
        split_point_x = x1 + (i / n) * (x2 - x1)
        split_point_y = y1 + (i / n) * (y2 - y1)
        equally_divided_points.append((split_point_x, split_point_y))

    equally_divided_points.append((x2, y2))
    # print(equally_divided_points)
    # print(len(equally_divided_points))
    return [equally_divided_points[idx] for idx in bl_time_tick]


def get_line_slope_and_intercept(line: t_line) -> tuple[float, float]:
    """return a line's slope and intercept"""
    x1, y1, x2, y2 = line
    slope = (y2 - y1) / (x2 - x1)
    intercept = y1 - slope * x1

    return slope, intercept


def calculate_y_coordinate(line: t_line, x: Union[int, float], find_x: bool = False) -> float:
    x1, y1, x2, y2 = line

    # Calculate the slope
    slope = (y2 - y1) / (x2 - x1)

    if find_x:
        # Calculate the x-axis value using the point-slope form
        v = x1 + (x - y1) / slope
        return v
    else:
        # Calculate the y-axis value using the point-slope form
        y = y1 + slope * (x - x1)
        return y


def get_orientation_between_points(a: t_point, b: t_point) -> float:
    """
    Get the orientation of the line which starts from point a and passes point b,
    with the updated orientation conventions.

    Args:
      a: The first point as a tuple (x1, y1).
      b: The second point as a tuple (x2, y2).

    Returns:
      The orientation of the line in degrees, in the range [0, 360), with the updated conventions.
    """

    # Calculate the difference between the x and y coordinates of the two points.
    dx = b[0] - a[0]
    dy = b[1] - a[1]

    # Calculate the angle of the line using the arctangent function.
    # atan2 returns the angle in radians between the positive x-axis and the point (x, y), but we need it from the y-axis.
    angle_radians = m.atan2(dx, dy)  # Swap dx and dy to calculate from y-axis and rotate the system

    # Convert the angle from radians to degrees.
    angle_degrees = m.degrees(angle_radians)

    # Normalize the angle to be in the range of [0, 360)
    angle_degrees = (angle_degrees + 360) % 360

    return angle_degrees


if __name__ == "__main__":
    print("Do not run this script directly")
